#include "subctrl.h"
#include "main.h"
#include "os_task.h"
#include "pid.h"
#include "spi.h"
#include "stream.h"
#include "sys.h"
#include <cmath>
#include <cstdint>
#include <cstring>
#include <string.h>

const static float kPhaseStep[] = {1.4f, 2.8f, 5.6f, 11.2f, 22.5f, 45.0f, 90.0f, 180.0f};

const static sub_control_flash_t kDefaultFlash = {
    .header = {.frame = RS485_FRAME_HEADER, .init = DEFAULT_INITIAL_VALUE, .ip = {192, 168, 11, 123}},
    .config =
        {
            .attenuation = {ATTENUATION_MAX, ATTENUATION_MAX, ATTENUATION_MAX},
        },
};

void SubController::Init(StreamProcessor *log)
{
    Log::Info("\n开始初始化子控制器");

    if (handle_.header.init != DEFAULT_INITIAL_VALUE) /* 硬件复位 */
    {
        memset(&handle_, 0, sizeof(sub_control_t));
        handle_.header.init = DEFAULT_INITIAL_VALUE;
        handle_.state.pa_enable = PA_MODULE_DISABLE_BIT;
        handle_.header.frame = RS485_FRAME_HEADER;

        handle_.state.soft_reset_count = 0;
        Log::Info("硬件复位，参数已初始化");
    }
    else /* 软件复位 */
    {
        handle_.state.soft_reset_count++;
        Log::Info("发生软件复位：%d次", handle_.state.soft_reset_count);
    }
    Log::Info("控制器状态：%s", GetStatus());
    Flash_Init(&flash_, &kDefaultFlash);
    LoadParameter();
    SetAttenuation(flash_.config.attenuation);
    SetPhase(flash_.config.phase);
    if (handle_.state.pa_enable != PA_MODULE_ENABLE_BIT)
        OutputDisable();
    // 加载pid参数
    // PID_Init(&forward_pid_, &handle_.flash.config.forward_pid);
    // PID_Init(&reverse_pid_, &handle_.flash.config.reverse_pid);
    Log::Info("子控制器初始化完成");
}

void SubController::ForwardLoop()
{
    if (handle_.state.auto_run != kON && handle_.state.pa_enable != PA_MODULE_ENABLE_BIT)
    {
        PID_Clear(&forward_pid_);
        return;
    }
    if (handle_.rf_power.reverse > REVERSE_ERROR_THRESHOLD)
        return;
    forward_pid_.error = handle_.rf_power.target - handle_.rf_power.forward;
    // handle_.flash.config.att_val -= PID_Update(&forward_pid_);
    // SetAttenuation(handle_.flash.config.att_val);
}
void SubController::ReverseLoop()
{
    static int8_t phase_dir = 1;
    static uint8_t anti_count = 0;

    if (handle_.state.auto_run != kON && handle_.state.pa_enable != PA_MODULE_ENABLE_BIT)
    {
        PID_Clear(&reverse_pid_);
    }
    /* 反向功率环 */
    if (reverse_pid_.error > reverse_pid_.error_prev) // 改变相位方向
    {
        anti_count++;
        if (anti_count >= PHASE_DIR_ANTI_COUNT)
        {
            anti_count = 0;
            phase_dir *= -1;
        }
    }
    else
    {
        anti_count = 0;
    }
    reverse_pid_.error = handle_.rf_power.reverse * phase_dir;
    // handle_.flash.config.phase_val += PID_Update(&reverse_pid_);
    // SetPhase(handle_.flash.config.phase_val);
}

void SubController::OutputEnable()
{
    handle_.state.pa_enable = PA_MODULE_ENABLE_BIT;

    OutputBitCtrl(handle_.state.pa_enable);
    Log::Info("输出使能");
}

void SubController::OutputEnable(pa_module_id_t &id)
{
    if (id >= kModuleNum)
    {
        Log::Info("模块ID: [%d] 超出范围", id);
        return;
    }
    BIT_SET(handle_.state.pa_enable, id);

    OutputBitCtrl(handle_.state.pa_enable);
    Log::Info("PA[%d] 输出使能", id);
}

void SubController::OutputDisable()
{
    handle_.state.pa_enable = PA_MODULE_DISABLE_BIT;

    OutputBitCtrl(handle_.state.pa_enable);
    Log::Info("输出失能");
}

void SubController::OutputDisable(pa_module_id_t &id)
{
    if (id >= kModuleNum)
    {
        Log::Info("模块ID: [%d] 超出范围", id);
        return;
    }
    BIT_CLEAR(handle_.state.pa_enable, id);

    OutputBitCtrl(handle_.state.pa_enable);
    Log::Info("PA[%d] 输出失能", id);
}

void SubController::SetTargetPower(int value)
{
    if (value < 0 || value > TARGET_POWER_MAX)
    {
        Log::Info("设置目标功率失败，超出范围");
        return;
    }
    handle_.rf_power.target = value;
    // PID_Init(&forward_pid_, &handle_.flash.config.forward_pid);
    // PID_Init(&reverse_pid_, &handle_.flash.config.reverse_pid);
    if (handle_.rf_power.target > 0)
    {
        Log::Info("设置目标功率为 %5.0f W, 并使能输出", handle_.rf_power.target);
        OutputEnable();
    }
    else
    {
        Log::Info("已失能输出");
        OutputDisable();
    }
}

void SubController::SetAttenuation(const pa_module_id_t &id, const float &dB)
{
    if (id >= kModuleNum)
    {
        Log::Error("模块ID: [%d] 超出范围", id);
        return;
    }
    if (dB < 0.0f || dB > ATTENUATION_MAX)
    {
        Log::Error("设置衰减值失败，超出范围");
        return;
    }

    if (HAL_DAC_SetValue(config_[id].dac_index->handle, config_[id].dac_index->channel, DAC_ALIGN_12B_R,
                         ((ATTENUATION_MAX - dB) / ATTENUATION_MAX) * 4095) == HAL_OK)
    {
        handle_.pa[id].attenuation = dB;
        Log::Info("设置PA [%d] 衰减值为 %5.2f dB", id, handle_.pa[id].attenuation);
    }
}

void SubController::SetAttenuation(float dB[kModuleNum])
{
    for (uint8_t i = 0; i < kModuleNum; i++)
    {
        SetAttenuation((pa_module_id_t &)i, dB[i]);
    }
}

void SubController::SetAttenuation(float pa1_dB, float pa2_dB, float pa3_dB)
{
    SetAttenuation(kModule1st, pa1_dB);
    SetAttenuation(kModule2nd, pa2_dB);
    SetAttenuation(kModule3rd, pa3_dB);
}

void SubController::SetAttenuation(uint8_t bit, int dB_step)
{
    float dB = 0;
    bit = bit & PA_MODULE_ALL_BIT;
    for (uint8_t i = 0; i < kModuleNum; i++)
    {
        if (BIT_READ(bit, i))
        {
            dB = handle_.pa[i].attenuation + dB_step * ATTENUATION_STEP;
            SetAttenuation((pa_module_id_t &)i, dB);
        }
    }
}

void SubController::SetPhase(const pa_module_id_t &id, const float &deg)
{
    static uint16_t data;
    if (id >= kModuleNum)
    {
        Log::Error("模块ID: [%d] 超出范围", id);
        return;
    }
    if (deg < 0.0f || deg > PHASE_MAX)
    {
        Log::Error("设置相位值失败，超出范围");
        return;
    }

    float actual = 0.0f;
    data = (uint16_t)roundf(deg * (256 / 360.0f));
    for (uint8_t i = 0; i < sizeof(kPhaseStep) / sizeof(float); i++)
    {
        if (BIT_READ(data, i))
            actual += kPhaseStep[i];
    }
    if (HAL_SPI_Transmit(&hspi1, (uint8_t *)&data, 1, HAL_MAX_DELAY) == HAL_OK)
    {
        handle_.pa[id].phase = actual;
        Log::Info("设置PA [%d] 相位值为 %5.2f 度", id, handle_.pa[id].phase);
    }
    PHASE_NSS_HIGH(config_[id].phase_index->port, config_[id].phase_index->pin);
    PHASE_NSS_LOW(config_[id].phase_index->port, config_[id].phase_index->pin);
}

void SubController::SetPhase(float deg[kModuleNum])
{
    for (uint8_t i = 0; i < kModuleNum; i++)
    {
        SetPhase((pa_module_id_t &)i, deg[i]);
    }
}

void SubController::SetPhase(float pa1_deg, float pa2_deg, float pa3_deg)
{
    SetPhase(kModule1st, pa1_deg);
    SetPhase(kModule2nd, pa2_deg);
    SetPhase(kModule3rd, pa3_deg);
}

void SubController::SetPhase(uint8_t bit, int deg_step)
{
    float deg = 0;
    bit = bit & PA_MODULE_ALL_BIT;
    for (uint8_t i = 0; i < kModuleNum; i++)
    {
        if (BIT_READ(bit, i))
        {
            deg = handle_.pa[i].phase + deg_step * PHASE_STEP;
            SetPhase((pa_module_id_t &)i, deg);
        }
    }
}

void SubController::SetAddress(uint8_t *addr)
{
    if (addr[3] < SUBCTRL_BASE_ADDRESS || addr[3] > (SUBCTRL_BASE_ADDRESS + kSubCtrlNum - 1))
    {
        Log::Error("设置IP地址失败! 地址应在 [%d, %d] 之间", SUBCTRL_BASE_ADDRESS,
                   SUBCTRL_BASE_ADDRESS + kSubCtrlNum - 1);
        return;
    }
    memcpy(handle_.header.ip, addr, 4);
    // clang-format off
    Log::Info("设置IP地址为 %d.%d.%d.%d",
                                handle_.header.ip[0],
                                handle_.header.ip[1],
                                handle_.header.ip[2],
                                handle_.header.ip[3]);
    // clang-format on
}

void SubController::SetAutoRun(switch_state_t &state)
{
    handle_.state.auto_run = state;
    Log::Info("设置自动运行状态为 %s", state ? "开启" : "关闭");
}

void SubController::SetForwardPID(pid_config_t &config)
{
    // Parameter check
    if (config.Kp > FORWARD_PID_KP_MAX || config.Ki > FORWARD_PID_KI_MAX || config.Kd > FORWARD_PID_KD_MAX ||
        config.output_max > FORWARD_PID_OUTPUT_MAX)
    {
        Log::Info("设置正向PID参数失败,超出范围");
        return;
    }
    // memcpy(&handle_.flash.config.forward_pid, &config, sizeof(pid_config_t));
    // PID_Init(&forward_pid_, &handle_.flash.config.forward_pid);
    Log::Info("已设置正向PID参数: ");
    PrintPidParam(forward_pid_);
}

void SubController::SetReversePID(pid_config_t &config)
{
    // Parameter check
    if (config.Kp > REVERSE_PID_KP_MAX || config.Ki > REVERSE_PID_KI_MAX || config.Kd > REVERSE_PID_KD_MAX ||
        config.output_max > REVERSE_PID_OUTPUT_MAX)
    {
        Log::Error("设置反向PID参数失败,超出范围");
        return;
    }
    // memcpy(&handle_.flash.config.reverse_pid, &config, sizeof(pid_config_t));
    // PID_Init(&reverse_pid_, &handle_.flash.config.reverse_pid);
    Log::Info("已设置反向PID参数: ");
    PrintPidParam(reverse_pid_);
}

char *SubController::GetStatus()
{
    static const char *status[] = {"正常", "异常", "重启"};
    return (char *)status[handle_.state.error];
}

uint8_t SubController::CheckAddress(uint8_t &addr)
{
    return handle_.header.ip[3] == addr;
}

sub_control_id_t SubController::GetDeviceID()
{
    sub_control_id_t id = (sub_control_id_t)(handle_.header.ip[3] - SUBCTRL_BASE_ADDRESS);
    if (id >= kSubCtrlNum)
        id = kSubCtrl1st;
    return id;
}

void SubController::PrintPidParam(pid_t &pid)
{
    Log::Info("Kp = %8.6f"
               "Ki = %8.6f"
               "Kd = %8.6f"
               "output_max = %5.2f"
               "output_min = %5.2f"
               "error_total_threshold = %5.2f"
               "error = %5.2f"
               "error_prev = %5.2f"
               "error_total = %5.2f",
               pid.config.Kp, pid.config.Ki, pid.config.Kd, pid.config.output_max, pid.config.output_min,
               pid.config.error_total_threshold, pid.error, pid.error_prev, pid.error_total);
}

void SubController::SaveParameter()
{
    memcpy(flash_.header.ip, handle_.header.ip, 4);
    for (uint8_t i = 0; i < kModuleNum; i++)
    {
        flash_.config.attenuation[i] = handle_.pa[i].attenuation;
        flash_.config.phase[i] = handle_.pa[i].phase;
    }
    if (Flash_Write(FLASH_START_ADDR, (uint32_t *)&flash_, sizeof(sub_control_flash_t) / sizeof(uint32_t)))
        Log::Error("参数保存失败");
    else
        Log::Info("参数保存成功");
}

void SubController::LoadParameter()
{
    memcpy(handle_.header.ip, flash_.header.ip, 4);
    for (uint8_t i = 0; i < kModuleNum; i++)
    {
        handle_.pa[i].attenuation = flash_.config.attenuation[i];
        handle_.pa[i].phase = flash_.config.phase[i];
    }
    Log::Info("已加载参数");
}

void SubController::ResetParameter()
{
    memcpy(&flash_, &kDefaultFlash, sizeof(sub_control_flash_t));
    Log::Info("已恢复默认参数");
    LoadParameter();
}

void SubController::Restart(uint8_t &mode)
{
    if (handle_.state.pa_enable != PA_MODULE_DISABLE_BIT)
    {
        Log::Error("请先关闭输出，再进行重启");
        return;
    }
    Log::Info("正在%s重启", mode == 1 ? "硬" : "软");
    handle_.header.init = mode;
    delay_ms(200);
    System_Reset();
}

// Private functions

void SubController::OutputBitCtrl(uint8_t bit)
{
    // clang-format off
    for (int8_t i = 0; i < kModuleNum; i++)
    {
        HAL_GPIO_WritePin(config_[i].pd_index->port,
                          config_[i].pd_index->pin,
                          (GPIO_PinState)BIT_READ(bit, i));
        delay_ms(20);
    }
    // clang-format on
}
